/**
*@note Copyright 2012-2022 CETHIK All Rights Reserved.
*@brief 一堆要用到的公式 
*@author tuyinan
*@data 2022-04-xx
*/

#include <costmap_2d/costmap_math.h>

void doTransform(costmap_2d::PointCloud* pt,
                 Eigen::Transform<float, 3, Eigen::Isometry> transform) {
  for (size_t i = 0; i < pt->points.size(); i++) {
    Eigen::Vector3f point =
        transform * Eigen::Vector3f((*pt).points[i].x, (*pt).points[i].y,
                                    (*pt).points[i].z);
    (*pt).points[i].x = point.x();
    (*pt).points[i].y = point.y();
    (*pt).points[i].z = point.z();
  }
}

double distanceToLine(double pX, double pY, double x0, double y0, double x1,
                      double y1) {
  double A = pX - x0;
  double B = pY - y0;
  double C = x1 - x0;
  double D = y1 - y0;

  double dot = A * C + B * D;
  double len_sq = C * C + D * D;
  double param = dot / len_sq;

  double xx, yy;

  if (param < 0) {
    xx = x0;
    yy = y0;
  } else if (param > 1) {
    xx = x1;
    yy = y1;
  } else {
    xx = x0 + param * C;
    yy = y0 + param * D;
  }

  return distance(pX, pY, xx, yy);
}